#include "config.h"
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <ctype.h>
#include <unistd.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/select.h>
#include <sys/signal.h>
#include <time.h>
#include <usb.h>
#include "garmin.h"

/* #if defined (HAVE_SYS_TERMIOS_H)
   #include <sys/termios.h>
#else
   #if defined (HAVE_TERMIOS_H) */
      #include <termios.h>
/*   #endif
#endif
*/
#define INTR_TIMEOUT  3000
#define BULK_TIMEOUT  3000

#ifndef TRUE
   #define TRUE 1
#endif

#ifndef FALSE
   #define FALSE 0
#endif

/* something magic about 64, garmin driver will not return more than
 * 64 at a time. If you read less than 64 bytes the next read will
 * just get the last of the 64 byte buffer.
 */
#define ASYNC_DATA_SIZE 64
#define PRIV_PKTID_SET_MODE     2
#define PRIV_PKTID_INFO_RESP    4
#define PRIV_PKTID_INFO_REQ     3
#define GARMIN_LAYERID_PRIVATE  0x01106E4B

const char *USBdevice = NULL;
int method = 0;		/* 0 = Read directly from USB-Device, 1 = read from special device */
struct termios ttyset, ttyset_old;
static fd_set all_fds;
void signal_handler_IO (int status);   /* definition of signal handler */
int wait_flag = TRUE;                    /* TRUE while no signal received */

int garmin_set_speed(garmin_unit *garmin, unsigned int speed, unsigned int stopbits);

void garmin_set_device(const char *device)
{
	USBdevice = device;
}

void garmin_set_method(int mth)
{
	method = (mth != 0) ? 1 : 0;
}

/* Close the USB connection with the Garmin device. */

int garmin_close(garmin_unit *garmin)
{
	if (!garmin)
	   return 1;

	if (method && garmin->usb.fd != -1)
	{
	   if (isatty(garmin->usb.fd))
	   {
	      /* force hangup on close on systems that don't do HUPCL properly */
	      cfsetispeed(&ttyset, (speed_t)B0);
	      cfsetospeed(&ttyset, (speed_t)B0);
	      tcsetattr(garmin->usb.fd, TCSANOW, &ttyset);
	   }

	   /* this is the clean way to do it */
	   ttyset_old.c_cflag |= HUPCL;
	   tcsetattr(garmin->usb.fd, TCSANOW, &ttyset_old);
	   close(garmin->usb.fd);
	   garmin->usb.fd = -1;
	   USBdevice = NULL;
	}
	else if (garmin->usb.handle != NULL )
	{
	   usb_release_interface(garmin->usb.handle,0);
	   usb_close(garmin->usb.handle);
	   garmin->usb.handle = NULL;
	}

	return 0;
}

/* 
   Open the USB connection with the first Garmin device we find.  Eventually,
   I'd like to add the ability to select a particular device.
*/

int garmin_open(garmin_unit *garmin)
{
struct usb_bus *     bi;
struct usb_device *  di;
int                  i;
static unsigned int rates[] = {0, 4800, 9600, 19200, 38400, 57600};
garmin_packet p;
struct sigaction saio;
char hv0[1024];
struct termios tp;

	if (!garmin)
	   return 0;

	if (method && garmin->usb.fd == -1)
	{
	   /* check for USB serial drivers -- very Linux-specific */
/*	   if (access("/sys/module/garmin_gps", R_OK) != 0)
	   {
	      garmin_queue_error("garmin_gps not active!", err_error);
	      return 0;
	   }
*/
	   if ((garmin->usb.fd = open(USBdevice, O_RDWR)) == -1)
	      return 0;

	   if (tcgetattr(garmin->usb.fd, &tp) == -1)
	      return 0;

	   tp.c_iflag &=
	     ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON|IXOFF|IXANY|INPCK|IUCLC);
	   tp.c_oflag &= ~OPOST;
	   tp.c_lflag &= ~(ECHO|ECHONL|ICANON|ISIG|IEXTEN|ECHOE);
	   tp.c_cflag &= ~(CSIZE|PARENB);
	   tp.c_cflag |= CS8 | CLOCAL | CREAD | CRTSCTS;

	   if (cfsetispeed(&tp, B115200) == -1)
	      return 0;

	   if (cfsetospeed(&tp, B115200) == -1)
	      return 0;
	
	   tp.c_cc[VMIN] = 1;
	   tp.c_cc[VTIME] = 0;

	   if (tcsetattr(garmin->usb.fd, TCSANOW, &tp) == -1)
	      return 0;

	   return 1;
	}
	else if (!method && garmin->usb.handle == NULL)
	{
	   usb_init();
	   usb_find_busses();
	   usb_find_devices();

	   for ( bi = usb_busses; bi != NULL; bi = bi->next )
	   {
	      for ( di = bi->devices; di != NULL; di = di->next )
	      {
	         if ( di->descriptor.idVendor  == GARMIN_USB_VID &&
			di->descriptor.idProduct == GARMIN_USB_PID )
		 {

		    if ( garmin->verbose != 0 )
		    {
		       sprintf(hv0, "[garmin] found VID %04x, PID %04x on %s/%s!",
			di->descriptor.idVendor,
			di->descriptor.idProduct,
			bi->dirname,
			di->filename);
		       garmin_queue_error(hv0, err_info);
		    }

		    garmin->usb.handle = usb_open(di);
		    garmin->usb.read_bulk = 0;

		    if ( garmin->usb.handle == NULL )
		    {
		       sprintf(hv0, "usb_open failed: %s!",usb_strerror());
		       garmin_queue_error(hv0, err_error);
		       return 0;
		    }

		    if (usb_set_configuration(garmin->usb.handle, 1) < 0 )
		    {
		       sprintf(hv0, "usb_set_configuration failed: %s!",usb_strerror());
		       garmin_queue_error(hv0, err_error);
		       return 0;
		    }

		    if ( usb_claim_interface(garmin->usb.handle,0) < 0 )
		    {
		       sprintf(hv0, "usb_claim_interface failed: %s!",usb_strerror());
		       garmin_queue_error(hv0, err_error);
		       return 0;
		    }

		    for ( i = 0; i < di->config->interface->altsetting->bNumEndpoints; i++ )
		    {
		     struct usb_endpoint_descriptor * ep;

		       ep = &di->config->interface->altsetting->endpoint[i];

		       switch ( ep->bmAttributes & USB_ENDPOINT_TYPE_MASK )
		       {
			  case USB_ENDPOINT_TYPE_BULK:
			     if ( ep->bEndpointAddress & USB_ENDPOINT_DIR_MASK )
			     {
			        garmin->usb.bulk_in = ep->bEndpointAddress & USB_ENDPOINT_ADDRESS_MASK;
			     }
			     else
			     {
			        garmin->usb.bulk_out = ep->bEndpointAddress & USB_ENDPOINT_ADDRESS_MASK;
			     }
			  break;

			  case USB_ENDPOINT_TYPE_INTERRUPT:
			     if ( ep->bEndpointAddress & USB_ENDPOINT_DIR_MASK )
			     {
			        garmin->usb.intr_in = ep->bEndpointAddress & USB_ENDPOINT_ADDRESS_MASK;
			     }
			  break;

			  default:
			     break;
		       }
		    }

		    break;
	         }
	      }

	      if ( garmin->usb.handle != NULL )
		 break;
	   }

	   if (garmin->usb.handle == NULL)
	      garmin_queue_error("Couldn't detect any Garmin device!", err_info);
	}

	return (garmin->usb.handle != NULL);
}

void signal_handler_IO (int status)
{
	wait_flag = FALSE;
}

int garmin_set_speed(garmin_unit *garmin, unsigned int speed, unsigned int stopbits)
{
unsigned int rate;

	if (!garmin)
	   return 0;

	if (speed < 300)
	   rate = B0;
	else if (speed < 1200)
	   rate = B300;
	else if (speed < 2400)
	   rate = B1200;
	else if (speed < 4800)
	   rate = B2400;
	else if (speed < 9600)
	   rate = B4800;
	else if (speed < 19200)
	   rate = B9600;
	else if (speed < 38400)
	   rate = B19200;
	else if (speed < 57600)
	   rate = B38400;
	else if (speed < 115200)
	   rate = B57600;
	else
	   rate = B115200;

	tcflush(garmin->usb.fd, TCIOFLUSH); /* toss stale data */

	if (rate != cfgetispeed(&ttyset) || stopbits != 1)
	{
	   cfsetispeed(&ttyset, (speed_t)rate);
	   cfsetospeed(&ttyset, (speed_t)rate);
	   ttyset.c_cflag &=~ CSIZE;
	   ttyset.c_cflag |= (CSIZE & (stopbits == 2 ? CS7 : CS8));

	   if (tcsetattr(garmin->usb.fd, TCSANOW, &ttyset) != 0)
	      return 0;

	   tcflush(garmin->usb.fd, TCIOFLUSH);
	}

/*	if ((session->packet_type = packet_sniff(session)) == BAD_PACKET)
	   return 0;
*/
	return 1;
}

uint8 garmin_packet_type(garmin_packet *p)
{
	if (!p) return 0;
	return p->packet.type;
}


uint16 garmin_packet_id(garmin_packet *p)
{
	if (!p) return 0;
	return get_uint16(p->packet.id);
}


uint32 garmin_packet_size(garmin_packet *p)
{
	if (!p) return 0;
	return get_uint32(p->packet.size);
}


uint8 *garmin_packet_data(garmin_packet *p)
{
	if (!p) return 0;
	return p->packet.data;
}


int garmin_packetize(garmin_packet *p, uint16 id, uint32 size, uint8 *data)
{
int ok = 0;

	if (!p) return 0;

	if (size + PACKET_HEADER_SIZE < sizeof(garmin_packet))
	{
	   p->packet.type       = GARMIN_PROTOCOL_APP;
	   p->packet.reserved1  = 0;
	   p->packet.reserved2  = 0;
	   p->packet.reserved3  = 0;
	   p->packet.id[0]      = id;
	   p->packet.id[1]      = id >> 8;
	   p->packet.reserved4  = 0;
	   p->packet.reserved5  = 0;
	   p->packet.size[0]    = size;
	   p->packet.size[1]    = size >> 8;
	   p->packet.size[2]    = size >> 16;
	   p->packet.size[3]    = size >> 24;

	   if ( size > 0 && data != NULL )
	   {
	      memcpy(p->packet.data,data,size);
	   }

	   ok = 1;
	}

	return ok;
}


int garmin_read(garmin_unit *garmin, garmin_packet *p)
{
int r = -1;
int cnt = 0;
int bLen = 0;
struct timespec delay, rem;
fd_set fds, rfds;
struct timeval tv;
int sel_ret = 0;
int ok = 0;
char hv0[1024];

	if (!garmin) return 0;

	if (garmin->usb.fd == -1 && garmin->usb.handle == NULL)
	   garmin_open(garmin);

	if (method && garmin->usb.fd != -1)
	{
	   long theBytesReturned = 0;
	   char *buf = (char *)p->data;
	   wait_flag = FALSE;

	   for(cnt = 0 ; cnt < 10 ; cnt++)
	   {
	      if (wait_flag != FALSE)
	      {
		 cnt--;
		 usleep (100000);
		 wait_flag = FALSE;
		 continue;
	      }

	      /* Read async data until the driver returns less than the
	       * max async data size, which signifies the end of a packet
	       * not optimal, but given the speed and packet nature of
	       * the USB not too bad for a start
	       */
	      wait_flag = TRUE;

	      theBytesReturned = read(garmin->usb.fd, buf + bLen, ASYNC_DATA_SIZE);

	      if (0 > theBytesReturned)
	      {
		 /* read error...
		  * or EAGAIN, but O_NONBLOCK is never set
		  */
		 sprintf(hv0, "Read error=%ld, errno=%d!", theBytesReturned, errno);
		 garmin_queue_error(hv0, err_error);
		 continue;
	      }

	      sprintf(hv0, "got %ld bytes.", theBytesReturned);
	      garmin_queue_error(hv0, err_info);
	      bLen += theBytesReturned;

	      if (theBytesReturned < ASYNC_DATA_SIZE)
	      {
		 /* zero length, or short, read is a flag for got the whole packet */
		 break;
	      }

	      if (256 <= bLen)
	      {
		 /* really bad read error... */
		 bLen = 0;
		 garmin_queue_error("Packet too long!", err_fatal);
		 break;
	      }

	      delay.tv_sec = 0;
	      delay.tv_nsec = 3330000L;

	      while (nanosleep(&delay, &rem) < 0)
	         continue;
	   }
	}
	else if (garmin->usb.handle != NULL)
	{
	   if (garmin->usb.read_bulk == 0)
	   {
	      r = usb_interrupt_read(garmin->usb.handle,
			     garmin->usb.intr_in,
			     p->data,
			     sizeof(garmin_packet),
			     INTR_TIMEOUT);
	      /* 
	       * If the packet is a "Pid_Data_Available" packet, we need to read
	       * from the bulk endpoint until we get an empty packet.
	       */

	      if (garmin_packet_type(p) == GARMIN_PROTOCOL_USB &&
			garmin_packet_id(p) == Pid_Data_Available)
	      {
		 /* FIXME!!! */
		 sprintf(hv0, "Received a Pid_Data_Available from the unit!");
		 garmin_queue_error(hv0, err_warning);
	      }
	   }
	   else
	   {
	      r = usb_bulk_read(garmin->usb.handle,
			garmin->usb.bulk_in,
			p->data,
			sizeof(garmin_packet),
			BULK_TIMEOUT);
	   }
	}

	if (garmin->verbose != 0 && r >= 0)
	{
	   garmin_print_packet(p, GARMIN_DIR_READ, stdout);
	}

	return r;
}


int garmin_write(garmin_unit *garmin, garmin_packet *p)
{
int r = -1;
int s = garmin_packet_size(p) + PACKET_HEADER_SIZE;

	if (!garmin)
	   return 0;

	if (garmin->usb.fd == -1 && garmin->usb.handle == NULL)
	   garmin_open(garmin);

	if (garmin->usb.handle != NULL || garmin->usb.fd != -1)
	{
	   if (garmin->verbose != 0)
	   {
	      garmin_print_packet(p, GARMIN_DIR_WRITE, stdout);
	   }

	   if (method)
	      r = write(garmin->usb.fd, p->data, s);
	   else
	      r = usb_bulk_write(garmin->usb.handle,
		       garmin->usb.bulk_out,
		       p->data,
		       s,
		       BULK_TIMEOUT);

	   if (r != s)
	   {
	   char hv0[1024];

	      if (method)
	      {
		 sprintf(hv0, "Write to device failed!");
		 garmin_queue_error(hv0, err_error);
	      }
	      else
	      {
		 sprintf(stderr, "usb_bulk_write failed: %s!", usb_strerror());
		 garmin_queue_error(hv0, err_error);
	      }

	      return 0;
	   }
	}

	return r;
}


uint32 garmin_start_session(garmin_unit *garmin)
{
garmin_packet p;
int i;
fd_set fds, rfds;
struct timeval tv;
int sel_ret = 0;
int ok = 0;

	if (!garmin)
	   return 0;

	garmin_packetize(&p, Pid_Start_Session, 0, NULL);
	p.packet.type = GARMIN_PROTOCOL_USB;

	if (method && garmin->usb.fd != -1)
	{
	   FD_ZERO(&fds);
	   FD_SET(garmin->usb.fd, &fds);
	}

	garmin_write(garmin, &p);
	garmin_write(garmin, &p);
	garmin_write(garmin, &p);

	if (garmin_read(garmin, &p) == 16)
	{
	   garmin->id = get_uint32(p.packet.data);
	}
	else
	{
	   garmin->id = 0;
	}

	garmin_callback("start_session");
	return garmin->id;
}


void garmin_print_packet(garmin_packet *p, int dir, FILE *fp)
{
int    i;
int    j;
uint32 s;
char   hex[128];
char   dec[128];

	if (!p)
	   return;

	s = garmin_packet_size(p);

	switch ( dir )
	{
	   case GARMIN_DIR_READ:   fprintf(fp,"<read");   break;
	   case GARMIN_DIR_WRITE:  fprintf(fp,"<write");  break;
	   default:                fprintf(fp,"<packet");        break;
	}

	fprintf(fp," type=\"0x%02x\" id=\"0x%04x\" size=\"%u\"",
	  garmin_packet_type(p),garmin_packet_id(p),s);

	if (s > 0)
	{
	   fprintf(fp,">\n");

	   for ( i = 0, j = 0; i < s; i++ )
	   {
	      sprintf(&hex[(3*(i&0x0f))]," %02x",p->packet.data[i]);
	      sprintf(&dec[(i&0x0f)],"%c",
		(isalnum(p->packet.data[i]) ||
		ispunct(p->packet.data[i]) ||
		p->packet.data[i] == ' ') ?
		p->packet.data[i] : '_');

	      if ( (i & 0x0f) == 0x0f )
	      {
		 j = 0;
		 fprintf(fp,"[%04x] %-54s %s\n",i-15,hex,dec);
	      }
	      else
	      {
		 j++;
	      }
	   }

	   if (j > 0)
	   {
	      fprintf(fp,"[%04x] %-54s %s\n",s-(s & 0x0f),hex,dec);
	   }

	   switch (dir)
	   {
	      case GARMIN_DIR_READ:   fprintf(fp,"</read>\n");   break;
	      case GARMIN_DIR_WRITE:  fprintf(fp,"</write>\n");  break;
	      default:                fprintf(fp,"</packet>\n"); break;
	   }
	}
	else
	{
	   fprintf(fp,"/>\n");
	}
}
